#include "calibration.h"

//编码器校正函数
void EncoderCalibrating(void)
{
    int32_t encoderVal = 0;
    int32_t currentEncoderVal = 0;
    int32_t lastEncoderVal = 0;
    int32_t iStart = 0;
    int32_t jStart = 0;
    int32_t stepNo = 0;
    // 一圈200整步的编码器值数组
    int32_t fullStepVal[MOTOR_STEPS_PER_REV];
    int32_t ticks = 0;
    // 写入位置索引
    uint32_t index = 0;
    uint16_t lookupAngle;
    dir = 1;
    DEBUG_port->printf("Calibrate Start!\r\n");
    // 开始时固定在整步位置
    Output(0, CALIBRATION_MODE_CURRENT);
    // 闪烁提示开始
    for (int16_t i = 0; i < 4; i++)
    {
        LED.set(HIGH);
        delay(250);
        LED.set(LOW);
        delay(250);
    }
    // 逆时针跑一圈记下各整步位的值
    DEBUG_port->printf("Anticlockwise Motion Start!\r\n");
    for (int16_t i = 0; i < MOTOR_STEPS_PER_REV; i++)
    {
        encoderVal = 0;
        lastEncoderVal = Encoder.ReadAngleHwCsYORO();
        // 多次读取取平均值
        for (uint8_t reading = 0; reading < 10; reading++)
        {
            currentEncoderVal = Encoder.ReadAngleHwCsYORO();
            // 溢出处理
            if (currentEncoderVal - lastEncoderVal < -(ENCODER_CNT_PER_REV / 2))
            {
                currentEncoderVal += ENCODER_CNT_PER_REV;
            }
            else if (currentEncoderVal - lastEncoderVal > (ENCODER_CNT_PER_REV / 2))
            {
                currentEncoderVal -= ENCODER_CNT_PER_REV;
            }
            encoderVal += currentEncoderVal;
            delay(2);
            lastEncoderVal = currentEncoderVal;
        }
        encoderVal = encoderVal / 10;
        // 溢出处理
        if (encoderVal > ENCODER_CNT_PER_REV)
        {
            encoderVal -= ENCODER_CNT_PER_REV;
        }
        else if (encoderVal < 0)
        {
            encoderVal += ENCODER_CNT_PER_REV;
        }
        fullStepVal[i] = encoderVal;
        OneStep();
    }
    dir = 0;
    OneStep();
    delay(1000);
    DEBUG_port->printf("Clockwise Motion Start!\r\n");
    for (int16_t i = (MOTOR_STEPS_PER_REV - 1); i >= 0; i--) //顺时针跑一圈记下各整步位的值和之前逆时针保存的值求平均
    {
        encoderVal = 0;
        lastEncoderVal = Encoder.ReadAngleHwCsYORO();
        for (uint8_t reading = 0; reading < 10; reading++)
        {
            currentEncoderVal = Encoder.ReadAngleHwCsYORO();
            if (currentEncoderVal - lastEncoderVal < -(ENCODER_CNT_PER_REV / 2))
            {
                currentEncoderVal += ENCODER_CNT_PER_REV;
            }
            else if (currentEncoderVal - lastEncoderVal > (ENCODER_CNT_PER_REV / 2))
            {
                currentEncoderVal -= ENCODER_CNT_PER_REV;
            }
            encoderVal += currentEncoderVal;
            delay(2);
            lastEncoderVal = currentEncoderVal;
        }
        encoderVal = encoderVal / 10;
        if (encoderVal > ENCODER_CNT_PER_REV)
        {
            encoderVal -= ENCODER_CNT_PER_REV;
        }
        else if (encoderVal < 0)
        {
            encoderVal += ENCODER_CNT_PER_REV;
        }
        fullStepVal[i] = (fullStepVal[i] + encoderVal) / 2;
        OneStep();
    }
    // 电桥电流置0
    // LL_TIM_OC_SetCompareCH1(TIM3, 0);
    // LL_TIM_OC_SetCompareCH2(TIM3, 0);
    CoilA.free();
    CoilB.free();
    DEBUG_port->printf("Save Cali Data Start!\r\n");
    for (uint8_t i = 0; i < MOTOR_STEPS_PER_REV; i++) //算出200整步间的插值数
    {
        ticks = fullStepVal[(i + 1) % MOTOR_STEPS_PER_REV] - fullStepVal[i % MOTOR_STEPS_PER_REV];
        if (ticks < -(ENCODER_CNT_PER_REV * 0.9))
        {
            ticks += ENCODER_CNT_PER_REV;
        }
        else if (ticks > (ENCODER_CNT_PER_REV * 0.9))
        {
            ticks -= ENCODER_CNT_PER_REV;
        }
        for (int32_t j = 0; j < ticks; j++)
        {
            stepNo = (fullStepVal[i] + j) % ENCODER_CNT_PER_REV;
            if (stepNo == 0)
            {
                iStart = i;
                jStart = j;
            }
        }
    }
    CaliTab.clear();
    for (int32_t i = iStart; i < (iStart + MOTOR_STEPS_PER_REV + 1); i++) //以步进电机整步为基准（小于0.08度）开始对编码器进行校正插值并将校正后的值存入FLASH
    {
        ticks = fullStepVal[(i + 1) % MOTOR_STEPS_PER_REV] - fullStepVal[i % MOTOR_STEPS_PER_REV];
        if (ticks < -(ENCODER_CNT_PER_REV * 0.9))
        {
            ticks += ENCODER_CNT_PER_REV;
        }
        if (i == iStart)
        {
            for (int32_t j = jStart; j < ticks; j++)
            {
                lookupAngle = ((ENCODER_CNT_PER_REV / 2) * i + (ENCODER_CNT_PER_REV / 2) * j / ticks) % (ENCODER_CNT_PER_REV * 100) / 100;
                CaliTab.set(index, (uint16_t)lookupAngle);
                index++;
            }
        }
        else if (i == (iStart + MOTOR_STEPS_PER_REV))
        {
            for (int32_t j = 0; j < jStart; j++)
            {
                lookupAngle = (((ENCODER_CNT_PER_REV / 2) * i + (ENCODER_CNT_PER_REV / 2) * j / ticks) % (ENCODER_CNT_PER_REV * 100)) / 100;
                CaliTab.set(index, (uint16_t)lookupAngle);
                index++;
            }
        }
        else
        {
            //this is the general case
            for (int32_t j = 0; j < ticks; j++)
            {
                lookupAngle = (((ENCODER_CNT_PER_REV / 2) * i + (ENCODER_CNT_PER_REV / 2) * j / ticks) % (ENCODER_CNT_PER_REV * 100)) / 100;
                CaliTab.set(index, (uint16_t)lookupAngle);
                index++;
            }
        }
    }
    if (!CaliTab.saveCaliTable())
    {
        DEBUG_port->println("Cali.saveCaliTable() feiled!");
    }
    DEBUG_port->printf("Calibrate End!\r\n");
    while (1)
    {
        LED.set(HIGH);
        delay(250);
        LED.set(LOW);
        delay(250);
    }
}